/**
 * Implement the primitive for geometric computation.
 * @author Songgang Xu (sxu at tamu dot edu)
 * @file primitive.h
 */

#ifndef _XMESH_PRIMITIVE_H
#define _XMESH_PRIMITIVE_H

#include <vector>

extern const double tiny_value ;// = 1.0e-5 ;

template <typename Value> class Vector   ;
template <typename Value> class Line     ;
template <typename Value> class Segment  ;
template <typename Value> class Ray      ;
template <typename Value> class Plane    ;
template <typename Value> class Polygon  ;

/**
 * The base class for all the primitives. 
 */
template <typename Value>
class Object
{
};
/*
template <typename Value>
class Point
{
	
};
    //----------------------------------------------------------
	// Comparison (dimension must match, or not)
	int operator==( Point);
	int operator!=( Point);
	//----------------------------------------------------------
	// Point and Vector Operations (always valid) 
	Vector operator-( Point);       // Vector difference
	Point  operator+( Vector);      // +translate
	Point  operator-( Vector);      // -translate
	Point& operator+=( Vector);     // inc translate
	Point& operator-=( Vector);     // dec translate
*/

/*
template <typename Value>
struct GeometryPrimitive
{
	typedef Vector<Value> VectorType ;
} ;
*/

template <typename Value>
class Line : public Object <Value>
{
protected:
	typedef Value ValueType ; 
	typedef typename Vector <Value>::PointType PointType ;
	typedef Vector <Value>            VectorType ; 
public:
	/**
	 * Default Constructor.
	 */
	Line () {}

	/**
	 * Copy Constructor.
	 * @param _l Line object. 
	 */
	Line (const Line & _l)  
		: m_point (_l.m_point), m_vector (_l.m_vector)
	{
	}
	
	/**
	 * Constructor. A line is defined by a point and a vector. 
	 * @param _p Point.
	 * @param _v Vector. 
	 */
	Line (const PointType & _p, const VectorType & _v) 
		: m_point (_p), m_vector (_v){}

public:
	/**
	 * Return const reference one point on the line. 
	 * @return Point. 
	 */
	const PointType & get_point   () const {return this->m_point; }

	/**
	 * Return reference one point on the line. 
	 * @return Point. 
	 */
	PointType & get_point   () {return this->m_point; }

	/**
	 * Return the direction of the line. 
	 * @return Vector of the direction. 
	 */ 
	const VectorType & get_vector () const {return this->m_vector ;}

	/**
	 * Return the direction of the line. 
	 * @return Vector of the direction. 
	 */ 
	VectorType & get_vector () {return this->m_vector ;}

	/**
	 * Return the nearest distant from the point to the line. 
	 */
	Value dist_from_point (PointType & _p) 
	{
		//Fix Me here. I didn't implement it.
		return Value(0);
	}

	/**
	 * Evaluate the point with paramater s.
	 * p + s * d. p is m_point, d is m_vector. 
	 */
	PointType evaluate (const ValueType & s)
	{
		return m_point + m_vector * s ;
	}

	/**
	 * Overload the operator << to output the line. 
	 * @param ostr Output stream. 
	 * @param _l const line object. 
	 */
	friend std::ostream & operator << (std::ostream & ostr, const Line & _l)
	{
		std::cout<<"Point "<<_l.m_point << " Direction " <<_l.m_vector ;
		return ostr ;
	}
	/*
	friend bool operator == (const Line & _lhs, const Line & _rhs)
	{
		if ( !(_lhs.m_vector == _rhs.m_vector)) return false ;
		VectorType v1 = _lhs.m_point - _rhs.m_point ;
		VectorType v2 = _rhs.m_point - _lhs.m_point ;


	}
	*/
private :
	PointType  m_point ;
	VectorType m_vector ;
} ;

/**
 * 3D Segment class. 
 */
template <typename Value>
class Segment : public Object<Value>
{
protected:
	typedef Value ValueType ; 
	typedef typename Vector <Value>::PointType PointType ;
	typedef Vector <Value>            VectorType ; 
	typedef Segment<Value>            SegmentType; 
	
public:
	Segment () {} ;
	Segment (const PointType &a, const PointType &b) : p0(a) , p1(b) { };
	PointType p0 ; 
	PointType p1 ;

	/**
	 *Return the square lenght of segment
	 */
	ValueType square_length ()
	{
		return (p0-p1).square_module();
	}
	/**
	 * Order two vertices in alphabetic order. 
	 */
	void order () 
	{
		if (!(p0 < p1)) {swap (p0, p1);}
	}

    /**
	 * Decide is point on the segment. 
	 * @param _p the point to be tested. 
	 */
	bool is_on (const PointType & _p) const
	{
		//VectorType d = ( p0 - _p ) ^ (p1 - _p) ;
		ValueType d = (p0 - _p) * (p1 - _p) ;

		if (d <= 0) return true; 
		// Guess if the intersection point are the ends. 
		if ((p0 - _p).square_module() < tiny_value)
		{
		   	return true; 
		}
	   	if ((p1 - _p).square_module() < tiny_value)
   		{
	   		return true; 
   		}		
		return false;
	}

	friend bool operator == (const SegmentType & seg1, const SegmentType & seg2)
	{
		return ((seg1.p0 == seg2.p0 && seg1.p1 == seg2.p1 )
			||  (seg1.p0 == seg2.p1 && seg1.p1 == seg2.p0 ));
	}

	friend bool operator != (const SegmentType & seg1, const SegmentType & seg2)
	{
		return (!(seg1 == seg2));
	}

	friend ostream & operator << (ostream & ostr, const Segment & seg)
	{
		ostr << seg.p0<<' '<<seg.p1 ;
		return ostr; 
	}
};


/**
 * 3D Ray class. 
 */
template <typename Value>
class Ray : public Object<Value>
{
protected:
	typedef Value ValueType ; 
	typedef typename Vector <Value>::PointType PointType ;
	typedef Vector <Value>            VectorType ; 
	typedef Segment<Value>            SegmentType; 

public :
	/**
	 * Default constructor.
	 */
	Ray () : m_point(VectorType (0,0,0)), m_dir (VectorType (0,0,0)){}

	/**
	 * Specify the start point and the direction.
	 * @param _p start point.
	 * @param _n the direction.
	 */
	Ray (const PointType & _p, const VectorType & _n) : m_point (_p), m_dir(_n)
	{
	}
	
	const PointType & get_point () const { return this->m_point; }
	const VectorType & get_direction() const {return this->m_dir ;}
	friend ostream & operator << (ostream & ostr, const Ray & ray )
	{
		ostr << ray.m_point << ray.m_dir ;
		return ostr; 
	}
private:
	PointType m_point ;
	VectorType m_dir; 
};

/**
 * 3D Polygon class. We assume this polygon is embedded in a plane. 
 */
template <typename Value>
class Polygon : public Object<Value>
{
protected:
	typedef Value ValueType ; 
	typedef typename Vector <Value>::PointType PointType ;
	typedef Vector <Value>            VectorType ; 
	typedef Plane <Value>            PlaneType ; 

public :

	/**
	 * Default constructor. 
	 */
	Polygon () {}

	/**
	 * Copy constructor. 
	 * @param _p Polygon object.
	 */
	Polygon (const Polygon & _p) 
		:m_vertices (_p.m_vertices){}

    /**
	 * Construct polygon from a point vector. 
	 */
	Polygon (const std::vector<PointType> & _vp) 
	{
		for (unsigned i = 0; i < _vp.size(); ++i)
		{
			this->m_vertices.push_back(_vp[i]);
		}
	}

	/**
	 * Return the number of vertices in polygon.
	 */
	unsigned size() const 
	{
		return this->m_vertices.size();
	}

    /**
	 * Construct polygon from a point vector. 
	 */
	Polygon (const std::vector<PointType *> & _vp) 
	{
		for (unsigned i = 0; i < _vp.size(); ++i)
		{
			this->m_vertices.push_back(*_vp[i]);
		}
	}

	/**
	 * Remove all the veritices in the polygon. 
	 * The polygon size will be zero.
	 */
	void clear(){this->m_vertices.clear();}

public:
	/**
	 * Decide if the point is inside of the polygon (convex or nonconvex). 
	 - Check if the point and the polygon are co-plane. 
	 - Map the point and the polygon to 2D domain. 
	 - Decide the relation in 2D space. 
	 @code 
    int pnpoly(int npol, float *xp, float *yp, float x, float y)
    {
      int i, j, c = 0;
      for (i = 0, j = npol-1; i < npol; j = i++) {
        if ((((yp[i] <= y) && (y < yp[j])) ||
             ((yp[j] <= y) && (y < yp[i]))) &&
            (x < (xp[j] - xp[i]) * (y - yp[i]) / (yp[j] - yp[i]) + xp[i]))
          c = !c;
      }
      return c;
    }
	@endcode
	 */
	bool is_on (const PointType & _p) const 
	{
		unsigned size = m_vertices.size();
		// Check if the point is on the vertex .
		for (unsigned i = 0; i < size; ++ i)
		{
			if ((m_vertices[i] - _p ).square_module() < tiny_value)
			{
				return true; 
			}
		}
		// Map the polygon and point to the 2D domain. 
		unsigned x, y;
		select_2Dmap_axis (x, y) ;
		
		bool rvalue = false; 
		for (unsigned i = 0, j = size - 1; i < size; j = i ++)
		{
			if ((((m_vertices[i][y] <= _p[y]) && (_p[y] < m_vertices[j][y])) ||
				 ((m_vertices[j][y] <= _p[y]) && (_p[y] < m_vertices[i][y]))) &&
				(_p[x] < (m_vertices[j][x] - m_vertices[i][x]) * (_p[y] - m_vertices[i][y]) / ( m_vertices[j][y] -  m_vertices[i][y]) + m_vertices[i][x]))
				rvalue = !  rvalue;  					
		}
		return rvalue; 
	}

	/**
	 * return the number of vertices.
	 */
	unsigned get_nb_vertices () const
	{
		return m_vertices.size();
	}

    /**
	 * set the number of vertices. 
	 * @param unsigned size; 
	 */
	void set_nb_vertices (unsigned _size) const
	{
		m_vertices.resize(_size);
	}

	/**
	 * return the l-value reference of vertices. 
	 */
	PointType & get_vertex (unsigned idx) 
	{
		return m_vertices[idx];
	}

    /**
	 * return the r-value reference of vertices. 
	 */
	const PointType & operator [](unsigned idx)  const
	{
		return m_vertices[idx];
	}
    /**
	 * return the l-value reference of vertices. 
	 */
	PointType & operator [](unsigned idx)  
	{
		return m_vertices[idx];
	}

	/**
	 * Calculate the normal of polygon.
	 * The vertices are ordered in counter-clockwise. 
	 - This function will not change the normal of this polygon. 
	 */
    VectorType calculate_normal () const 
	{
		if (m_vertices.size() <3) throw ("A face with less than 3 vertices does not have a normal") ;
		return   (m_vertices[1] - m_vertices[0]) ^ (m_vertices[2] - m_vertices[0]) ;
	}


	// Get/Set the normal of the polygon
    /**
     * Returns the normal of this polygon.
     * For performance reason, the normal is precalculated.
     * @return The normal of the polygon.
     */
    const VectorType & get_normal() const 
	{
		return this->m_normal; 
	}

    /**
     * Sets the normal for this polygon.
     * @param normal The normal of the polygon.
     */
    void set_normal(const VectorType & _n) { this->m_normal = _n; }

	/**
	 * Get the plane that the polygon embedded. 
	 * pre {update_normal()}
	 */
	const PlaneType get_plane () const
	{

		return PlaneType (m_vertices[0], this->m_normal );
	}

	/**
	 * Get the area of the polygon. 
	 * Refer to 
	 * @link 
	 http://mathworld.wolfram.com/PolygonArea.html 
	 Area 
	 * @endlink
	 * @return the area of the polygon. It is always non-negative. 
	 * @pre {The function update_normal() is called. }
	 */
	Value get_area () 
	{
		VectorType normal = m_normal ;
		normal.normalize();

		ValueType area = ValueType(0);
		ValueType an = normal.module();
		ValueType ax, ay, az ;
		unsigned coord ; // coord to be ignored. 1-x, 2-y, 3-z
		
		ax = abs(normal[0]) ;
		ay = abs(normal[1]) ;
		az = abs(normal[2]) ;
		
		coord = 2; 

		if (ax > ay )
		{
			if (ax > az) coord = 0;
		}
		else
		{
			if (ay > az) coord = 1 ;
		}


		unsigned size = m_vertices.size();
		for (unsigned k = 0; k < size ; ++k )
		{
			unsigned i = (k + 1) % size  ;
			switch (coord)
			{
			case 0:
				area  += (m_vertices[k][1] * m_vertices[i][2] - m_vertices[k][2] * m_vertices[i][1] );
				break; 
			case 1:
				area  += (m_vertices[k][0] * m_vertices[i][2] - m_vertices[k][2] * m_vertices[i][0] );
				break; 
			case 2:
				area  += (m_vertices[k][0] * m_vertices[i][1] - m_vertices[k][1] * m_vertices[i][0] );
				break; 
			}
		}
		switch (coord) {
		case 0:
			area *= (an / (2*ax));
			break;
		case 1:
			area *= (an / (2*ay));
			break;
		case 2:
			area *= (an / (2*az));
		}
		return abs(area) ;
	}

	void append_vertex (const PointType & _v)
	{
		this->m_vertices.push_back(_v);
	}
	friend ostream & operator << (ostream & ostr, const Polygon & _p)
	{
		for (unsigned i = 0;i  < _p.m_vertices.size(); ++i)
		{
			ostr<< _p.m_vertices [i] <<' ';
		}
		return ostr; 
	}
protected :
	/**
	 * Decide along which axis to do 2D map. 
	 * @return 0-x, 1-y, 2-z; 
	 */
	unsigned select_2Dmap_axis(unsigned & x, unsigned & y) const 
	{
		VectorType normal = m_normal; 
		normal.normalize();

//		ValueType area = ValueType(0);
//		ValueType an = normal.module();
		ValueType ax, ay, az ;
		unsigned coord ; // coord to be ignored. 1-x, 2-y, 3-z
		
		ax = abs(normal[0]) ;
		ay = abs(normal[1]) ;
		az = abs(normal[2]) ;
		
		coord = 2; 

		if (ax > ay )
		{
			if (ax > az) coord = 0;
		}
		else
		{
			if (ay > az) coord = 1 ;
		}


		switch (coord)
		{
		case 0:
			x = 1; y = 2; 
			break; 
		case 1:
			x = 2; y = 0; 
			break; 
		case 2:
			x = 0; y = 1;
			break; 
		}


		return coord; 
	}
protected :
	std::vector <PointType> m_vertices ;
	VectorType m_normal;
};



/**
 * 3D Plane class. 
 * A plane is defined by ax + by + cz + d = 0. 
 */
template <typename Value>
class Plane : public Object<Value>
{
protected:
	typedef Value ValueType ; 
	typedef typename Vector <Value>::PointType PointType ;
	typedef Vector <Value>            VectorType ; 

public :

    /**
     * Constructs an uninitialised plane.
     */
    Plane() {}

	/**
	 * Constucts a plane with the point _p and the normal _n
	 * @param _p One point on the plane.
	 * @param _n The positive direction of plane. 
	 */
	Plane (const PointType & _p, const VectorType & _n)
	{
		fromPointAndNormal(_p, _n);
	}

    /**
     * Constructs an initialised plane.
     * @param a The a factor.
     * @param b The b factor.
     * @param c The c factor.
     * @param d The d factor.
     */
    Plane(double _a, double _b, double _c, double _d) : a(_a), b(_b), c(_c), d(_d) {}

    /**
     * Constructs a plane from three points.
     * @param p0 1th point.
     * @param p1 2nd point.
     * @param p2 3th point.
     */
    void fromPoints(const PointType &p0, const PointType &p1, const PointType &p2)
	{
		VectorType v0 (p0 - p1);
		VectorType v1 (p2 - p1);
		VectorType n = v1 ^ v0 ;
		n.normalize();
		a = n.x ;
		b = n.y ;
		c = n.z ;
		d = -(p0.x * a + p0.y * b + p0.z * c);
	}

    /**
     * Constructs a plane from three points and don't normalize the equation.
     * @param p0 1th point.
     * @param p1 2nd point.
     * @param p2 3th point.
     */
    void fromPointsNN(const PointType &p0, const PointType &p1, const PointType &p2)
	{
		VectorType v0 (p0 - p1);
		VectorType v1 (p2 - p1);
		VectorType n = v1 ^ v0 ;
		a = n.x ;
		b = n.y ;
		c = n.z ;
		d = -(p0.x * a + p0.y * b + p0.z * c);		
	}

    /**
     *  Constructs a plane from a point and a normal.
     * @param p The point that lies on the plane.
     * @param n The normal of the plane.
     */
    void fromPointAndNormal(const PointType &p0, const VectorType &n)
	{
		VectorType normal = n ;
		normal.normalize();
		a = normal[0] ;
		b = normal[1] ;
		c = normal[2] ;
		d = -(p0[0] * a + p0[1] * b + p0[2] * c); 
	}

    /**
     * Makes a dot product between a point and the plane. Result becomes closer to d's factor as the point is near to the plane.
     * @param p The point.
     * @return The dot product.
     */
    ValueType dot(const VectorType &p) const
    {
		return a * p.x + b * p.y + c * p.z;
    }

    /**
     * Calculates the distance between a point and the plane.
     * @param p The point.
     * @return The distance.
     */
    double dist(const VectorType &p) const
    {
		return a * p[0] + b * p[1] + c * p[2] + d;
    }

    /**
     * Reflects a vector on the plane.
     * @param p The vector to be reflected.
     * @return The reflected vector.
     */
    VectorType reflect(const VectorType &vec) const 
    {
        ValueType d = dist(vec);
        return  vec + VectorType(-a, -b, -c) * d * 2.0f;
    }

    /**
     * Projects a point onto the plane.
     * @param p The point to be projected.
     * @returned the projected point.
     */
	VectorType project(const VectorType &p) const
    {
        double h = dist(p);
        return VectorType(   p[0] - a * h,
						  p[1] - b * h,
						  p[2] - c * h);
    }

    /**
     * Tells if a point is on the plane or not.
     * @param p The point to test.
     * @param (optional) The threshold to use to determine if the point is on the plane or not. Indeed we couldn't just test if the distance from the point to the plane is zero because of potential numerical error. Default is 10^-3.
     * @return true if the point is on the plane, false otherwhise.
     */
    bool isOnPlane(const VectorType &p, double threshold = tiny_value)
    {
        double d = dist(p);
        if (d < threshold && d > -threshold)
            return true;
        return false;
    }

    /**
     * Determines if a line and the place intersect.
     * @param p0 The first point of the line.
     * @param p1 The second point of the line.
     * @param t A value that will be filled with the distance to travel to get the intersection.
     * @return If the plane and the line are parallel false is returned, true otherwhise since the line obviously intersects with the plane.
     */
    bool intersectWithLine(const VectorType &p0, const VectorType &p1, double &t)
    {
        VectorType dir = p1 - p0;
        double div = dot(dir);
        if (div == 0)
            return false;

		t = -dist(p0) / div;
        return true;
    }


	/**
	 * Get parameters. 
	 */
	void get_parameter (ValueType & _a, ValueType & _b, ValueType & _c, ValueType & _d) const 
	{
		_a = a; _b = b ; _c = c; _d = d; 
	}
private :
	/**A plane is defined by a x + b y + c z + d = 0*/
	ValueType a , b , c , d; 
};


template <typename Value>
class Sphere : public Object<Value>
{
};

template <typename Value>
class Ellipsoid : public Object<Value>
{
};

enum PLANE
{
	LEFT_PLANE , RIGHT_PLANE, BOTTOM_PLANE, TOP_PLANE, NEAR_PLANE, FAR_PLANE
};



/**
 * Box class. Axias aligned box. 
 */
template <typename Value>
class Box : public Object<Value>
{
protected :
	typedef Value ValueType ; 
	typedef typename Vector <Value>::PointType PointType ;
	typedef Vector <Value>                     VectorType ; 
	typedef Plane <Value>                      PlaneType ;
	typedef Polygon<Value>                     PolygonType ;
	/**
	 * We use segment to show the edges of box. 
	 */
	typedef Segment<Value>                     BoxEdgeType ;
public: 
	Box () {}
	Box (const VectorType & _c, const VectorType & _e) : center (_c), extent (_e) {}
	Box (const PolygonType & _p) 
	{
		throw ("Incomplete function. Contruct box from polygon.");
	}
	~Box (){}

	/**
	 * Get the minimum coordinate vertex. 
	 */
	VectorType getMin() const { return center - extent ;}

	/**
	 * Get the maximum coordinate vertex. 
	 */
	VectorType getMax() const { return center + extent ;}

	/**
	 * Get the center of the box. 
	 */
	VectorType getCenter () const {return this->center ;}

	/**
	 * Get the extent of the box. 
	 */
	VectorType getExtent () const {return this->extent ;}

	/**
	 * Return the volumn of the box. 
	 */
	ValueType get_volumn () const 
	{
		return abs (ValueType(8) * extent[0] * extent[1] * extent[2] ) ;
	}

	void setFromPoints(const VectorType &_pt0, const VectorType &_pt1)
    {
        center = (_pt1 + _pt0) * ValueType(0.5);
        extent = (_pt1 - _pt0) * ValueType(0.5);
        extent.positive();
    }
	
	bool isOverlap (const Box & _box) const 
	{ 
		return isOverlapX(_box) && isOverlapY (_box) && isOverlapZ(_box) ;
	}
	bool isInside(const Box &_box) const
	{
		return (center.x - extent.x > _box.center.x - _box.extent.x &&
			center.x + extent.x < _box.center.x + _box.extent.x &&
			center.y - extent.y > _box.center.y - _box.extent.y &&
			center.y + extent.y < _box.center.y + _box.extent.z &&
			center.z - extent.z > _box.center.z - _box.extent.z &&
				center.z + extent.z < _box.center.z + _box.extent.z) ;
	}
	bool isInside (const VectorType & _pt) const 
	{
		VectorType min = getMin();
		VectorType max = getMax ();
		return (_pt[0] >= min[0] && _pt[0] <= max[0] 
			&& _pt[1] >= min[1] && _pt[1] <= max[1]
			&& _pt[2] >= min[2] && _pt[2] <= max[2]) ;
	}
	VectorType getVertex (int _index) const 
	{
		switch(_index) 
		{
		case 0: return VectorType(center.x - extent.x, center.y - extent.y, center.z - extent.z);
		case 1: return VectorType(center.x + extent.x, center.y - extent.y, center.z - extent.z);
		case 2: return VectorType(center.x - extent.x, center.y + extent.y, center.z - extent.z);
		case 3: return VectorType(center.x + extent.x, center.y + extent.y, center.z - extent.z);
		case 4: return VectorType(center.x - extent.x, center.y - extent.y, center.z + extent.z);
		case 5: return VectorType(center.x + extent.x, center.y - extent.y, center.z + extent.z);
		case 6: return VectorType(center.x - extent.x, center.y + extent.y, center.z + extent.z);
		case 7: return VectorType(center.x + extent.x, center.y + extent.y, center.z + extent.z);
		default:
			return VectorType ( 0.0f, 0.0f, 0.0f);
		}
	}

	static VectorType getFaceNormal (PLANE _plane)
	{
		switch(_plane)
		{
		case LEFT_PLANE : return VectorType( -1.0f, 0.0f, 0.0f);
		case RIGHT_PLANE: return VectorType( 1.0f, 0.0f, 0.0f);
		case BOTTOM_PLANE: return VectorType( 0.0f, -1.0f, 0.0f);
		case TOP_PLANE:return VectorType( 0.0f, 1.0f, 0.0f);
		case NEAR_PLANE:return VectorType( 0.0f, 0.0f, -1.0f);
		case FAR_PLANE:return VectorType( 0.0f, 0.0f, 1.0f);
		default:
			return VectorType(0.0f, 0.0f, 0.0f); 
		}
	}

	PolygonType getFacePolygon (int _idx) const
	{
		switch (_idx)
		{
		case 0 : break;
		case 1 : break;
		case 2 : break;
		case 3 : break;
		case 4 : break;
		case 5 : break;
		case 6 : break;
		}
	}
	PlaneType getPlane(PLANE _which) const
    {
        PlaneType plane;
        switch(_which) {
            case 0: plane.fromPointAndNormal(VectorType(center.x - extent.x, 0, 0), getFaceNormal(_which));
            case 1: plane.fromPointAndNormal(VectorType(center.x + extent.x, 0, 0), getFaceNormal(_which));
            case 2: plane.fromPointAndNormal(VectorType(0, center.y - extent.y, 0), getFaceNormal(_which));
            case 3: plane.fromPointAndNormal(VectorType(0, center.y + extent.y, 0), getFaceNormal(_which));
            case 4: plane.fromPointAndNormal(VectorType(0, 0, center.z - extent.z), getFaceNormal(_which));
            case 5: plane.fromPointAndNormal(VectorType(0, 0, center.z + extent.z), getFaceNormal(_which));
            default: plane = PlaneType(0, 0, 0, 0);
        }
        return plane;
    }

    BoxEdgeType getEdge(int _index) const
    {
        ValueType x0 = center[0] - extent[0];
        ValueType y0 = center[1] - extent[1];
        ValueType z0 = center[2] - extent[2];
        ValueType x1 = center[0] + extent[0];
        ValueType y1 = center[1] + extent[1];
        ValueType z1 = center[2] + extent[2];

        switch(_index) {
            case 0: return BoxEdgeType(VectorType(x0, y0, z0), VectorType(x1, y0, z0));
            case 1: return BoxEdgeType(VectorType(x1, y0, z0), VectorType(x1, y1, z0));
            case 2: return BoxEdgeType(VectorType(x1, y1, z0), VectorType(x0, y1, z0));
            case 3: return BoxEdgeType(VectorType(x0, y1, z0), VectorType(x0, y0, z0));

            case 4: return BoxEdgeType(VectorType(x0, y0, z1), VectorType(x1, y0, z1));
            case 5: return BoxEdgeType(VectorType(x1, y0, z1), VectorType(x1, y1, z1));
            case 6: return BoxEdgeType(VectorType(x1, y1, z1), VectorType(x0, y1, z1));
            case 7: return BoxEdgeType(VectorType(x0, y1, z1), VectorType(x0, y0, z1));

            case 8: return BoxEdgeType(VectorType(x0, y0, z0), VectorType(x0, y0, z1));
            case 9: return BoxEdgeType(VectorType(x1, y0, z0), VectorType(x1, y0, z1));
            case 10: return BoxEdgeType(VectorType(x1, y1, z0), VectorType(x1, y1, z1));
		    case 11: return BoxEdgeType(VectorType(x0, y1, z0), VectorType(x0, y1, z1));

            default: return BoxEdgeType(VectorType(0.0f, 0.0f, 0.0f), VectorType(0.0f, 0.0f, 0.0f));
        }
    }

protected:
	bool isOverlapX (const Box & _box) const
	{
		double t = _box.center[0] - center[0] ;
		t = t <0 ? (-t) : t ;
		double ext = _box.extent[0] + extent[0] ;
		if (t <= ext) return true; 
		return false; 
	}
	bool isOverlapY (const Box & _box) const
	{
		double t = _box.center[1] - center[1] ;
		t = t <0 ? (-t) : t ;
		double ext = _box.extent[1] + extent[1] ;
		if (t <= ext) return true; 
		return false; 
	
	}
	bool isOverlapZ (const Box & _box) const 
	{
		double t = _box.center[2] - center[2] ;
		t = t <0 ? (-t) : t ;
		double ext = _box.extent[2] + extent[2] ;
		if (t <= ext) return true; 
		return false; 
	
	}
public :
	VectorType center ;
	VectorType extent ;
};

/**
 * Defines an OBB (Oriented Bounding Box) which is merely an oriented AABB.
 */
template <typename Value>
class OBB : public Box <Value>
{
protected:
	typedef DenseMatrix<Value> MatrixType ;
public:
	
    MatrixType matrix; /**< The rotation matrix that affects the Box to obtain that OBB.*/
};


enum POSITION
{
	NONINTERSECT, OVERLAP, INTERSECT
};

enum PRIMITIVE_TYPE
{
	POINT, SEGMENT, LINE, RAY, POLYGON, PLANE
}; 

template <typename Value>
struct IntersectResult
{
	POSITION pos ;
	PRIMITIVE_TYPE res_type ;


	Vector<Value>   point ;
	Segment <Value> segment ;
	Line <Value>    line ;
	Polygon <Value> polygon ;
	Plane <Value>   plane ;

	friend std::ostream & operator << (std::ostream & ostr, const IntersectResult & res )
	{
		switch (res.pos)
		{
		case OVERLAP: std::cout<<"overlap"<<std::endl; break;
		case NONINTERSECT : std::cout<<"NONintersect"<<std::endl; break;
		case INTERSECT : std::cout<<"Intersect"<<std::endl; break;;
		}

		if (res.pos == INTERSECT || res.pos == OVERLAP)
		{
			switch (res.res_type)
			{
			case POINT: std::cout<<"Point : " << res.point << std::endl; break; 
			case SEGMENT: std::cout<<"Segment : " << res.segment<< std::endl; break; 
			case LINE: std::cout<<"Line : " << res.line<< std::endl; break; 
			case POLYGON: std::cout<<"Polygon : " << res.polygon<< std::endl; break; 
			}
		}
		return ostr ;
	}
};

#endif
